# Untitled - By: Swiper_Witty - 周六 11月 27 2021

import sensor
import image
import lcd
import time
from Maix import GPIO
from machine import UART
from fpioa_manager import fm

fm.register(11,fm.fpioa.UART2_TX)
fm.register(7,fm.fpioa.UART2_RX)
uart_B = UART(UART.UART2, 19200, 8, 0, 0, timeout=1000, read_buf_len=4096)

fm.register(13,fm.fpioa.GPIO1)
fm.register(12,fm.fpioa.GPIO2)
fm.register(14,fm.fpioa.GPIO3)
LED_R = GPIO(GPIO.GPIO1,GPIO.OUT)
LED_G = GPIO(GPIO.GPIO2,GPIO.OUT)
LED_B = GPIO(GPIO.GPIO3,GPIO.OUT)

write_str = '{K210:Ready}'
timer = 0
clock = time.clock()
lcd.init()

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.run(1)

red_threshold   = (0, 100, 6, 62, 10, 69)
green_threshold   = (0, 100, -50, -12, -27, 28)
blue_threshold   = (0, 100, -25, 40, -51, -8)
bx_num = 0
by_num = 0
Tip_num = 0

def LED_RGB(A = ' '):
    if A == 'r' or  A == 'R':
        LED_R.value(0)
        LED_G.value(1)
        LED_B.value(1)
    elif A == 'g' or  A == 'G':
        LED_R.value(1)
        LED_G.value(0)
        LED_B.value(1)
    elif A == 'b' or  A == 'B':
        LED_R.value(1)
        LED_G.value(1)
        LED_B.value(0)
    else:
        LED_R.value(1)
        LED_G.value(1)
        LED_B.value(1)
    return

time.sleep_ms(200)
LED_RGB()

while True:
    clock.tick()
    img=sensor.snapshot()                               # get photo
    fps =clock.fps()

    if Tip_num == 1:
        blobs = img.find_blobs([red_threshold])           # find color
        if blobs :
            for b in blobs:
                if(b.pixels() > 20000):                     # 部分的像素数量
                    tmp=img.draw_rectangle(b[0:4])          # 矩形框标识
                    tmp=img.draw_cross(b[5], b[6])          # 绘制十字
                    img.draw_cross(b.cx(), b.cy())          #
                    bx_num = b.cx()                         # 坐标
                    LED_RGB('R')
                    first = red_threshold
                    Tip_num = 2
                    print("OK")

        blobs = img.find_blobs([green_threshold])
        if blobs :
            for b in blobs:
                if(b.pixels() > 20000):
                    tmp=img.draw_rectangle(b[0:4])
                    tmp=img.draw_cross(b[5], b[6])
                    img.draw_cross(b.cx(), b.cy())
                    bx_num = b.cx()
                    LED_RGB('G')
                    first = green_threshold
                    Tip_num = 2
                    print("OK")

        blobs = img.find_blobs([blue_threshold])
        if blobs :
            for b in blobs:
                if(b.pixels() > 20000):
                    tmp=img.draw_rectangle(b[0:4])
                    tmp=img.draw_cross(b[5], b[6])
                    img.draw_cross(b.cx(), b.cy())
                    bx_num = b.cx()
                    LED_RGB('B')
                    first = blue_threshold
                    Tip_num = 2
                    print("OK")

    elif Tip_num == 2 and timer > 32:
        Tip_num = 2
        blobs = img.find_blobs([first])         # 第二次颜色
        if blobs :
            for b in blobs:

                if(b.pixels() > 10000):
                    tmp=img.draw_rectangle(b[0:4])
                    tmp=img.draw_cross(b[5], b[6])
                    img.draw_cross(b.cx(), b.cy())
                    bx_num = b.cx()
                    if bx_num >= 188 and t == 0:            #超过188的X坐标 而且只 Stop 一次
                        uart_B.write('{K210:Stop}')
                        t = 1
                    print(bx_num)

    if uart_B.any():
        read_data = uart_B.read()
        read_str = read_data.decode('utf-8')
        print(read_str)
        time.sleep_ms(50)
        if 'Start-K210' in read_str:                        #单片机复位 视觉
            Tip_num = 1
            t = 0                                           #只发一次 Stop 标记
            timer = 0

            LED_RGB()
            print('K210:Ready')
            uart_B.write('{K210:Ready}')
            time.sleep_ms(50)
        elif 'Stop-K210' in read_str:
            Tip_num = 0
            print('K210 Stop')
        else:
            print('UART2 EEROR')
            time.sleep_ms(50)

    if Tip_num == 2:
        timer = timer + 1
    time.sleep_ms(10)
